import numpy as np
from scipy.optimize import differential_evolution
from scipy.integrate import simpson
import matplotlib.pyplot as plt
import pandas as pd

# 常量定义
g = 9.8  # 重力加速度
v_smoke_sink = 3  # 烟幕云团下沉速度
smoke_effective_radius = 10  # 烟幕有效遮蔽半径
smoke_duration = 20  # 烟幕有效持续时间

# 初始位置
true_target = np.array([0, 200, 0])  # 真目标中心
fake_target = np.array([0, 0, 0])  # 假目标中心
M1_start = np.array([20000, 0, 2000])  # M1导弹初始位置
FY1_start = np.array([17800, 0, 1800])  # FY1无人机初始位置

# 导弹参数
v_missile = 300  # 导弹速度
missile_direction = fake_target - M1_start  # 导弹方向向量
missile_direction = missile_direction / np.linalg.norm(missile_direction)  # 单位化

# 无人机速度范围
v_drone_min = 70
v_drone_max = 140

# 时间参数
t_max = 67  # 导弹飞行总时间（从初始位置到假目标）
time_step = 0.1  # 时间步长
t_eval = np.arange(0, t_max, time_step)  # 评估时间点


def missile_position(t):
    """计算导弹在时间t的位置"""
    return M1_start + v_missile * missile_direction * t


def drone_position(v, theta, t):
    """计算无人机在时间t的位置"""
    x = FY1_start[0] + v * np.cos(theta) * t
    y = FY1_start[1] + v * np.sin(theta) * t
    z = FY1_start[2]  # 等高度飞行
    return np.array([x, y, z])


def smoke_trajectory(v, theta, t_drop, t, g=9.8):
    """计算烟幕弹在时间t的位置（投放后）"""
    # 投放时的无人机位置
    drop_pos = drone_position(v, theta, t_drop)

    # 投放后的时间
    t_after_drop = t - t_drop
    if t_after_drop < 0:
        return drop_pos  # 尚未投放

    # 水平速度（与无人机相同）
    vx = v * np.cos(theta)
    vy = v * np.sin(theta)

    # 位置：水平匀速，垂直自由落体（初始垂直速度为0）
    x = drop_pos[0] + vx * t_after_drop
    y = drop_pos[1] + vy * t_after_drop
    z = drop_pos[2] - 0.5 * g * t_after_drop ** 2

    return np.array([x, y, z])


def smoke_cloud_position(smoke_pos, t_burst, t):
    """计算烟幕云团在时间t的位置"""
    if t < t_burst:
        return smoke_pos  # 尚未起爆

    # 起爆后的时间
    t_after_burst = t - t_burst

    # 云团位置（水平位置不变，垂直位置匀速下沉）
    x = smoke_pos[0]
    y = smoke_pos[1]
    z = smoke_pos[2] - v_smoke_sink * t_after_burst

    return np.array([x, y, z])


def distance_point_to_line_segment(point, line_start, line_end):
    """计算点到线段的最短距离"""
    # 线段向量
    line_vec = line_end - line_start
    line_len = np.linalg.norm(line_vec)
    line_unitvec = line_vec / line_len

    # 点到线段起点的向量
    point_vec = point - line_start

    # 点在线段上的投影长度
    projection_len = np.dot(point_vec, line_unitvec)

    # 如果投影在线段外，计算到端点的距离
    if projection_len < 0:
        return np.linalg.norm(point - line_start)
    elif projection_len > line_len:
        return np.linalg.norm(point - line_end)

    # 否则计算垂足距离
    projection_point = line_start + projection_len * line_unitvec
    return np.linalg.norm(point - projection_point)


def is_effective(smoke_cloud_pos, missile_pos, target_pos):
    """判断烟幕是否有效遮蔽：导弹与目标之间的连线穿过烟幕云团10m范围内"""
    # 计算云团中心到导弹-目标连线的最短距离
    distance = distance_point_to_line_segment(smoke_cloud_pos, missile_pos, target_pos)

    # 判断距离是否小于有效半径
    return distance < smoke_effective_radius


def calculate_individual_effectiveness(v, theta, t_d, t_b, t_eval):
    """计算单个烟幕弹的有效遮蔽时间"""
    effective_mask = np.zeros_like(t_eval, dtype=bool)

    for i, t in enumerate(t_eval):
        if t >= t_d:  # 烟幕弹已投放
            m_pos = missile_position(t)
            smoke_pos = smoke_trajectory(v, theta, t_d, t)
            cloud_pos = smoke_cloud_position(smoke_pos, t_b, t)

            if t_b <= t <= t_b + smoke_duration and is_effective(cloud_pos, m_pos, true_target):
                effective_mask[i] = True

    return simpson(effective_mask.astype(float), t_eval)


def objective_function(params):
    """目标函数：计算总有效遮蔽时间"""
    # 解析参数
    v, theta, t_d1, t_d2, t_d3, delta1, delta2, delta3 = params

    # 计算起爆时间
    t_b1 = t_d1 + delta1
    t_b2 = t_d2 + delta2
    t_b3 = t_d3 + delta3

    # 初始化遮蔽标志数组
    effective_mask = np.zeros_like(t_eval, dtype=bool)

    # 对每个时间点计算是否被遮蔽
    for i, t in enumerate(t_eval):
        # 计算导弹位置
        m_pos = missile_position(t)

        # 计算三个烟幕弹的位置
        # 烟幕弹1
        if t >= t_d1:
            smoke_pos1 = smoke_trajectory(v, theta, t_d1, t)
            cloud_pos1 = smoke_cloud_position(smoke_pos1, t_b1, t)
            if t_b1 <= t <= t_b1 + smoke_duration and is_effective(cloud_pos1, m_pos, true_target):
                effective_mask[i] = True
                continue

        # 烟幕弹2
        if t >= t_d2:
            smoke_pos2 = smoke_trajectory(v, theta, t_d2, t)
            cloud_pos2 = smoke_cloud_position(smoke_pos2, t_b2, t)
            if t_b2 <= t <= t_b2 + smoke_duration and is_effective(cloud_pos2, m_pos, true_target):
                effective_mask[i] = True
                continue

        # 烟幕弹3
        if t >= t_d3:
            smoke_pos3 = smoke_trajectory(v, theta, t_d3, t)
            cloud_pos3 = smoke_cloud_position(smoke_pos3, t_b3, t)
            if t_b3 <= t <= t_b3 + smoke_duration and is_effective(cloud_pos3, m_pos, true_target):
                effective_mask[i] = True

    # 计算总有效遮蔽时间
    total_effective_time = simpson(effective_mask.astype(float), t_eval)

    # 添加惩罚项（确保投放时间间隔至少1秒）
    penalty = 0
    if t_d2 < t_d1 + 1:
        penalty += 100 * (t_d1 + 1 - t_d2)
    if t_d3 < t_d2 + 1:
        penalty += 100 * (t_d2 + 1 - t_d3)

    # 添加惩罚项（确保起爆时间在合理范围内）
    if t_b1 > t_max or t_b2 > t_max or t_b3 > t_max:
        penalty += 100

    # 目标是最大化遮蔽时间，因此返回负值（用于最小化算法）
    return -(total_effective_time - penalty)


def optimize_smoke_strategy():
    """优化烟幕投放策略"""
    # 定义参数边界
    bounds = [
        (v_drone_min, v_drone_max),  # v
        (0, 2 * np.pi),  # theta
        (0, t_max / 3),  # t_d1
        (0, 2 * t_max / 3),  # t_d2
        (0, t_max),  # t_d3
        (1, 20),  # delta1
        (1, 20),  # delta2
        (1, 20)  # delta3
    ]

    # 使用差分进化算法进行优化
    result = differential_evolution(
        objective_function,
        bounds,
        strategy='best1bin',
        maxiter=200,  # 增加迭代次数
        popsize=20,  # 增加种群大小
        tol=0.001,
        mutation=(0.5, 1),
        recombination=0.7,
        seed=42
    )

    return result


# 运行优化
if __name__ == "__main__":
    print("开始优化烟幕投放策略...")
    result = optimize_smoke_strategy()

    # 提取最优参数
    v_opt, theta_opt, t_d1_opt, t_d2_opt, t_d3_opt, delta1_opt, delta2_opt, delta3_opt = result.x
    t_b1_opt = t_d1_opt + delta1_opt
    t_b2_opt = t_d2_opt + delta2_opt
    t_b3_opt = t_d3_opt + delta3_opt

    # 计算最优总遮蔽时间
    optimal_time = -result.fun

    # 计算每个烟幕弹的单独遮蔽时间
    time1 = calculate_individual_effectiveness(v_opt, theta_opt, t_d1_opt, t_b1_opt, t_eval)
    time2 = calculate_individual_effectiveness(v_opt, theta_opt, t_d2_opt, t_b2_opt, t_eval)
    time3 = calculate_individual_effectiveness(v_opt, theta_opt, t_d3_opt, t_b3_opt, t_eval)

    print(f"优化完成！最优总遮蔽时间: {optimal_time:.2f} 秒")
    print(f"烟幕弹1单独遮蔽时间: {time1:.2f} 秒")
    print(f"烟幕弹2单独遮蔽时间: {time2:.2f} 秒")
    print(f"烟幕弹3单独遮蔽时间: {time3:.2f} 秒")
    print(f"无人机速度: {v_opt:.2f} m/s")
    print(f"无人机方向: {np.degrees(theta_opt):.2f} 度")
    print(f"烟幕弹1投放时间: {t_d1_opt:.2f} s, 起爆时间: {t_b1_opt:.2f} s")
    print(f"烟幕弹2投放时间: {t_d2_opt:.2f} s, 起爆时间: {t_b2_opt:.2f} s")
    print(f"烟幕弹3投放时间: {t_d3_opt:.2f} s, 起爆时间: {t_b3_opt:.2f} s")

    # 计算投放点和起爆点坐标
    drop_point1 = drone_position(v_opt, theta_opt, t_d1_opt)
    drop_point2 = drone_position(v_opt, theta_opt, t_d2_opt)
    drop_point3 = drone_position(v_opt, theta_opt, t_d3_opt)

    burst_point1 = smoke_trajectory(v_opt, theta_opt, t_d1_opt, t_b1_opt)
    burst_point2 = smoke_trajectory(v_opt, theta_opt, t_d2_opt, t_b2_opt)
    burst_point3 = smoke_trajectory(v_opt, theta_opt, t_d3_opt, t_b3_opt)

    # 保存结果到Excel文件
    data = {
        '无人机运动方向': [np.degrees(theta_opt), np.degrees(theta_opt), np.degrees(theta_opt), ''],
        '无人机运动速度 (m/s)': [v_opt, v_opt, v_opt, ''],
        '烟幕干扰弹编号': [1, 2, 3, '总有效遮蔽时间'],
        '烟幕干扰弹投放点的x坐标 (m)': [drop_point1[0], drop_point2[0], drop_point3[0], ''],
        '烟幕干扰弹投放点的y坐标 (m)': [drop_point1[1], drop_point2[1], drop_point3[1], ''],
        '烟幕干扰弹投放点的z坐标 (m)': [drop_point1[2], drop_point2[2], drop_point3[2], ''],
        '烟幕干扰弹起爆点的x坐标 (m)': [burst_point1[0], burst_point2[0], burst_point3[0], ''],
        '烟幕干扰弹起爆点的y坐标 (m)': [burst_point1[1], burst_point2[1], burst_point3[1], ''],
        '烟幕干扰弹起爆点的z坐标 (m)': [burst_point1[2], burst_point2[2], burst_point3[2], ''],
        '有效干扰时长 (s)': [time1, time2, time3, optimal_time]
    }

    df = pd.DataFrame(data)
    df.to_excel('result.xlsx', index=False)
    print("结果已保存到 result.xlsx")

    # 可视化遮蔽效果
    effective_mask = np.zeros_like(t_eval, dtype=bool)
    for i, t in enumerate(t_eval):
        m_pos = missile_position(t)

        # 烟幕弹1
        if t >= t_d1_opt:
            smoke_pos1 = smoke_trajectory(v_opt, theta_opt, t_d1_opt, t)
            cloud_pos1 = smoke_cloud_position(smoke_pos1, t_b1_opt, t)
            if t_b1_opt <= t <= t_b1_opt + smoke_duration and is_effective(cloud_pos1, m_pos, true_target):
                effective_mask[i] = True
                continue

        # 烟幕弹2
        if t >= t_d2_opt:
            smoke_pos2 = smoke_trajectory(v_opt, theta_opt, t_d2_opt, t)
            cloud_pos2 = smoke_cloud_position(smoke_pos2, t_b2_opt, t)
            if t_b2_opt <= t <= t_b2_opt + smoke_duration and is_effective(cloud_pos2, m_pos, true_target):
                effective_mask[i] = True
                continue

        # 烟幕弹3
        if t >= t_d3_opt:
            smoke_pos3 = smoke_trajectory(v_opt, theta_opt, t_d3_opt, t)
            cloud_pos3 = smoke_cloud_position(smoke_pos3, t_b3_opt, t)
            if t_b3_opt <= t <= t_b3_opt + smoke_duration and is_effective(cloud_pos3, m_pos, true_target):
                effective_mask[i] = True

    plt.figure(figsize=(10, 6))
    plt.plot(t_eval, effective_mask.astype(float), 'b-', label='有效遮蔽')
    plt.xlabel('时间 (s)')
    plt.ylabel('遮蔽状态')
    plt.title('烟幕遮蔽效果随时间变化')
    plt.legend()
    plt.grid(True)
    plt.savefig('smoke_effectiveness.png')
    plt.show()